/**
* \brief 
* \author pengcheng (pengcheng@yslrpch@126.com)
* \date 2020-06-09
* \attention CopyrightÃ‚Â©ADC Technology(tianjin)Co.Ltd
* \attention Refer to COPYRIGHT.txt for complete terms of copyright notice
*/

#include "cmw_app/detection_centernet_app.h"
#include <yaml-cpp/yaml.h>
#include "common/project_root.h"

namespace adc
{
DetectionCenternet::DetectionCenternet(const std::string& config_file, const float hz) : Worker(hz)
{
    InitConfig(config_file);
    detection_callback_ = nullptr;
    detection_img_callback_ = nullptr;
}

void DetectionCenternet::CacheImages(const cv::Mat& img, TimeType& tm)
{
    std::lock_guard<std::mutex> mutex_cache_img(mutex_image_);
    image_cache_.push(img.clone());
    time_cache_.push(tm);
    while(image_cache_.size() > kQueueSize)
    {
        image_cache_.pop();
        time_cache_.pop();
    }
}

bool DetectionCenternet::ReceiveImage(void)
{
    std::lock_guard<std::mutex> mutex_cache_img(mutex_image_);
    if(image_cache_.empty()) return false;
    current_img_ = image_cache_.front();
    current_time_ = time_cache_.front();
    image_cache_.pop();
    time_cache_.pop();
    return true;

}
void DetectionCenternet::SetDetectionImgCallback(DetectionImgCallback callback)
{
    detection_img_callback_ = callback;
}


void DetectionCenternet::SetDetectionCallback(DetectionCallback callback)
{
    detection_callback_ = callback;
}

void DetectionCenternet::DoWork()
{
    static int count = 0;
    if(!ReceiveImage())
    {
        count++;
        if(count > 30)
        {
            LOG(ERROR)<<"center net app can not recive img";
        }
        return;
    }
    count = 0;
    std::vector<cv::Mat> imgs;
    imgs.push_back(current_img_);
    res_bbox_.clear();
    center_net_ptr_->Detect(imgs, res_bbox_);

    if(detection_callback_)
    {
        detection_callback_(res_bbox_[0], current_time_);
    }

    if(detection_img_callback_)
    {
        detection_img_callback_(res_bbox_[0], current_img_, current_time_);
    }

}

void DetectionCenternet::InitConfig(const std::string& config_file)
{
    try
    {   
        std::string f = config_file;
        YAML::Node node = YAML::LoadFile(f);
        std::string center_net_config = adc::RootPath::GetAbsolutePath(node["center_net_config"].as<std::string>());
        center_net_ptr_ = std::make_shared<detection::CenterNetDetector<float>>(center_net_config);
    }
    catch(const std::exception& e)
    {
        LOG(ERROR)<<"Init yaml file, Faile :"<<config_file;
        std::cerr << e.what() << '\n';
    }
    
}
}